#include "ShiftSuck.h"

auto ss_pos_pid = MM::PIDInstance(6.0f, 0.0f, 0.0f, 2000.0f, 1000.0f, 10.0f);
auto ss_pos_array = std::array<MM::PIDInstance, 8>{ss_pos_pid, ss_pos_pid};

auto ss_vel_pid = MM::PIDInstance(5.0f, 0.3f, 0.0f, 15000.0f, 1000.0f, 10.0f);
auto ss_vel_array = std::array<MM::PIDInstance, 8>{ss_vel_pid, ss_vel_pid};


ShiftSuck &ShiftSuck::getInstance() {
    static ShiftSuck _instance;
    return _instance;
}

void ShiftSuck::posVelControl(const float shiftPos, const float shiftVel, const float suckPos,
                              const float suckVel) {
    m3508s.posVelControl({-shiftPos, +shiftPos, -suckPos, +suckPos},
                         {-shiftVel, +shiftVel, -suckVel, +suckVel}, ss_pos_array, ss_vel_array);
}

ShiftSuck::ShiftSuck(): m3508s("can0", "shift&suck", true, false) {
    m3508s.enable();
}
